#include "Plane.h"
#include <iostream>
#include <vector>
#include <algorithm>
#include "cv.h"
#include "highgui.h"
#include <Eigen/LU>
#include <Eigen/Eigenvalues> 
#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <Eigen/Core>

using namespace Eigen;
using namespace std;
int plane_counter = 0;

Plane::Plane(Point * a, Point * b,Point * c)
{
	id = plane_counter++;
	use_boundaries = false;
	setup(a, b, c);
}

Plane::Plane(std::vector<Point *> * points)
{
	id = plane_counter++;
	use_boundaries = false;
	setup(points);
}

Plane::Plane()
{
	id = plane_counter++;
	use_boundaries = false;
}

Plane::~Plane()
{}

float ** getDataFromVector(std::vector<Point *> * points)
{
	
	int size = points->size();
	//ROS_INFO("size: %i",size);
	float ** retval = new float*[3];
	retval[0] = new float[size];
	retval[1] = new float[size];
	retval[2] = new float[size];
	
	for(int i = 0; i < size; i++)
	{
		Point * p = points->at(i);
		retval[0][i] = p->x;
		retval[1][i] = p->y;
		retval[2][i] = p->z;
	}
	return retval;
}

float * getMean(float ** data, int nr_data)
{
	float mx = 0;
	float my = 0;
	float mz = 0;
	
	for(int i = 0; i < nr_data; i++)
	{
		mx+=data[0][i];
		my+=data[1][i];
		mz+=data[2][i];
	}
	float * retval = new float[3];
	retval[0] = mx / float(nr_data);
	retval[1] = my / float(nr_data);
	retval[2] = mz / float(nr_data);
	return retval;
}

MatrixXf getCov(float ** data, int nr_data , float * mean)
{
	MatrixXf covMat(3,3);
	for(int i = 0; i < 3; i++)
	{
		for(int j = 0; j <= i; j++)
		{
			float * col1 	= data[i];
			float mean1 	= mean[i];
			float * col2 	= data[j];
			float mean2 	= mean[j];
			float sum = 0;
			for(int k = 0; k < nr_data; k++)
			{
				sum+=(col1[k]-mean1)*(col2[k]-mean2);
			}
			covMat(i,j)=sum/float(nr_data-1);
			covMat(j,i)=covMat(i,j);
		}
	}
	return covMat;
}

void Plane::setup(std::vector<Point *> * points)
{
	
	int size = points->size();
	float ** data = getDataFromVector(points);
	float *  mean = getMean(data,size);
	MatrixXf covMat = getCov(data, size , mean);
	JacobiSVD<MatrixXf> svd(covMat, ComputeThinU | ComputeThinV);

	MatrixXf U = svd.matrixU();
	U = -U;	
	point_x = mean[0];
	point_y = mean[1];
	point_z = mean[2];
	mean[0] = 0;
	mean[1] = 0;
	mean[2] = 0;
	
	plane_points(0,0) 	= 		 mean[0];
	plane_points(0,1) 	= 		 mean[1];
	plane_points(0,2) 	= 		 mean[2];
	
	plane_points(1,0) 	= U(0,0)+mean[0];
	plane_points(1,1) 	= U(1,0)+mean[1];
	plane_points(1,2) 	= U(2,0)+mean[2];
	
	plane_points(2,0) 	= U(0,1)+mean[0];
	plane_points(2,1) 	= U(1,1)+mean[1];
	plane_points(2,2) 	= U(2,1)+mean[2];
	
	normal_x 			= U(0,2);
	normal_y 			= U(1,2);
	normal_z 			= U(2,2);
	
	delete mean;
	delete data[0];
	delete data[1];
	delete data[2];
	delete data;
	getABCDcoefs();
}

void Plane::getABCDcoefs()
{
	//ROS_INFO("Mean: %f %f %f",point_x,point_y,point_z);
	Matrix3f plane_points_mean;
	plane_points_mean(0,0) 	= plane_points(0,0)+point_x;
	plane_points_mean(0,1) 	= plane_points(0,1)+point_y;
	plane_points_mean(0,2) 	= plane_points(0,2)+point_z;
	
	plane_points_mean(1,0) 	= plane_points(1,0)+point_x;
	plane_points_mean(1,1) 	= plane_points(1,1)+point_y;
	plane_points_mean(1,2) 	= plane_points(1,2)+point_z;
	
	plane_points_mean(2,0) 	= plane_points(2,0)+point_x;
	plane_points_mean(2,1) 	= plane_points(2,1)+point_y;
	plane_points_mean(2,2) 	= plane_points(2,2)+point_z;
	
	Matrix3f a_det_points;
	a_det_points(0,0) 	= 1;
	a_det_points(0,1) 	= plane_points(0,1)+point_y;
	a_det_points(0,2) 	= plane_points(0,2)+point_z;
	
	a_det_points(1,0) 	= 1;
	a_det_points(1,1) 	= plane_points(1,1)+point_y;
	a_det_points(1,2) 	= plane_points(1,2)+point_z;
	
	a_det_points(2,0) 	= 1;
	a_det_points(2,1) 	= plane_points(2,1)+point_y;
	a_det_points(2,2) 	= plane_points(2,2)+point_z;
	
	Matrix3f b_det_points;
	b_det_points(0,0) 	= plane_points(0,0)+point_x;
	b_det_points(0,1) 	= 1;
	b_det_points(0,2) 	= plane_points(0,2)+point_z;
	
	b_det_points(1,0) 	= plane_points(1,0)+point_x;
	b_det_points(1,1) 	= 1;
	b_det_points(1,2) 	= plane_points(1,2)+point_z;
	
	b_det_points(2,0) 	= plane_points(2,0)+point_x;
	b_det_points(2,1) 	= 1;
	b_det_points(2,2) 	= plane_points(2,2)+point_z;
	
	Matrix3f c_det_points;
	c_det_points(0,0) 	= plane_points(0,0)+point_x;
	c_det_points(0,1) 	= plane_points(0,1)+point_y;
	c_det_points(0,2) 	= 1;
	
	c_det_points(1,0) 	= plane_points(1,0)+point_x;
	c_det_points(1,1) 	= plane_points(1,1)+point_y;
	c_det_points(1,2) 	= 1;
	
	c_det_points(2,0) 	= plane_points(2,0)+point_x;
	c_det_points(2,1) 	= plane_points(2,1)+point_y;
	c_det_points(2,2) 	= 1;
	
	//cout << "plane_points:\n"<<plane_points_mean<<endl;
	float D = plane_points_mean.determinant();
	//cout << "D = " << D <<endl;
	d = 100.0f;
	a = -(d/D) *a_det_points.determinant();
	b = -(d/D) *b_det_points.determinant();
	c = -(d/D) *c_det_points.determinant();
	//cout << "a_det_points:\n"<<a_det_points<<endl;
	//cout << "b_det_points:\n"<<b_det_points<<endl;
	//cout << "c_det_points:\n"<<c_det_points<<endl;
}

void Plane::setup(Point * a, Point * b,Point * c)
{
	Eigen::Vector3f x1(b->x - a->x,b->y - a->y,b->z - a->z);
	Eigen::Vector3f x2(c->x - a->x,c->y - a->y,c->z - a->z);
	Eigen::Vector3f nhat_top = x1.cross(x2);
	Eigen::Vector3f nhat = nhat_top / nhat_top.norm();
	normal_x = nhat(0);
	normal_y = nhat(1);
	normal_z = nhat(2);
	point_x = a->x;
	point_y = a->y;
	point_z = a->z;
}

inline float Plane::distance(Point * point){
	if(!use_boundaries && point != 0){
		return normal_x*(point_x-point->x) + normal_y*(point_y-point->y) + normal_z*(point_z-point->z);
	}else{
		return 99999999;
	}
}

void Plane::project_points(std::vector<Point *> points){
	float top = normal_x*point_x + normal_y*point_y + normal_z*point_z;
	int size = points.size();
	for(int i = 0; i < size; i++)
	{
		Point * p = points.at(i);
		float d = top/(normal_x*p->x+normal_y*p->y+normal_z*p->z);
		p->x*=d;
		p->y*=d;
		p->z*=d;
		p->pos(0) = p->x;
		p->pos(1) = p->y;
		p->pos(2) = p->z;
	}
}

bool compare_vec(std::vector<Point *> * a,std::vector<Point *> * b){return a->size()>b->size();}

std::vector< Plane *> * Plane::extract_subplanes(std::vector<Point *> * points){
	int nr_points = points->size();
	//ROS_INFO("nr points to extract subplanes from:%i",nr_points);
	int   max_width 	=  0;
	int   max_height 	=  0;
	for(unsigned int i = 0; i < nr_points; i++)
	{
		Point * p = points->at(i);
		if(max_width < p->w)	{max_width = p->w;}
		if(max_height < p->h)	{max_height = p->h;}
	}
	
	Point *** pointMat = new Point**[max_width];
	for(int i = 0; i < max_width; i++)
	{
		pointMat[i] = new Point*[max_height];
		for(int j = 0; j < max_height; j++)
		{
			pointMat[i][j] = 0;
		}
	}
	
	for(unsigned int i = 0; i < nr_points; i++)
	{
		Point * p = points->at(i);
		pointMat[p->w-1][p->h-1] = p;
	}
	
	std::vector< std::vector<Point *> *> * components = new std::vector< std::vector<Point *> *>();

	for(int i = 0; i < max_width; i++)
	{
		for(int j = 0; j < max_height; j++)
		{
			if(pointMat[i][j] != 0)
			{
				std::vector<Point *>* component = new std::vector<Point *>();
				std::vector<Point *>* todo 		= new std::vector<Point *>();
				components						->push_back(component);
				todo							->push_back(pointMat[i][j]);
				pointMat[i][j] 					= 0;
				while(!todo->empty())
				{
					Point * p = todo->back();
					todo->pop_back();
					component->push_back(p);
					int w = p->w-1;
					int h = p->h-1;
					
					int a_start = w-1;
					if(a_start < 0)				{a_start = 0;}
					int a_stop = w+1;
					if(a_stop > max_width-1)	{a_stop = max_width-1;}
					
					int b_start = h-1;
					if(b_start < 0)				{b_start = 0;}
					int b_stop = h+1;
					if(b_stop > max_height-1)	{b_stop = max_height-1;}
					
					for(int a = a_start; a <= a_stop;a++)
					{
						for(int b = b_start; b <= b_stop;b++)
						{ 
							if(pointMat[a][b]!=0){
								todo->push_back(pointMat[a][b]);
								pointMat[a][b]=0;
							}
						}
					}
				}
				delete todo;
			}
		}
	}
	
	for(int i = 0; i < max_width; i++){delete pointMat[i];}
	delete pointMat;
	
	//ROS_INFO("components->size() = %i",(int)components->size());
	std::sort(components->begin(), components->end(), compare_vec);
	std::vector< Plane *> * sub_planes = new std::vector< Plane *>();
	
	Plane * pl = new Plane(components->at(0));
	//pl->setBoundaries(components->at(0));
	sub_planes->push_back(pl);
	
	/*
	for(unsigned int i = 0; i < components->size();i++)
	{
		std::vector<Point *> * component = components->at(i);
		int size = component->size();
		ROS_INFO("components[%i]->size() = %i",(int)i,size);
		if(size > 100)
		{
			Plane * pl = new Plane(component);
			sub_planes->push_back(pl);
		}
		
	}
	*/
    return sub_planes;
}

void Plane::setBoundaries(std::vector<Point *> * points){
	float top = normal_x*point_x + normal_y*point_y + normal_z*point_z;
	int size = points->size();
	ROS_INFO("TOP: %f, size: %i",top,size);
	float * U_pos = new float[size];
	float * V_pos = new float[size];
	float x1 = plane_points(1,0);
	float x2 = plane_points(2,0);
	float x3 = plane_points(0,0);
	
	float y1 = plane_points(1,1);
	float y2 = plane_points(2,1);
	float y3 = plane_points(0,1);
	
	float z1 = plane_points(1,2);
	float z2 = plane_points(2,2);
	float z3 = plane_points(0,2);
	//float v_bot = 
	
	float max_u = -1000000;
	float min_u = 1000000;
	
	float max_v = -1000000;
	float min_v = 1000000;
	for(int i = 0; i < size; i++)
	{
		Point * p = points->at(i);
		float d = top/(normal_x*p->x+normal_y*p->y+normal_z*p->z);
		float x_pos = p->x*d;
		float y_pos = p->y*d;
		float z_pos = p->z*d;
		
		float a = x_pos;
		float b = y_pos;
		
		float v =  (b-y3-(y1/x1)*(a-x3))/(y2- (y1*x2/x1));
		float u =  ((a-x3)/x1) - (x2*v/x1);
		U_pos[i] = u;
		V_pos[i] = v;
		if(v > max_v){max_v = v;}
		if(u > max_u){max_u = u;}
		
		if(v < min_v){min_v = v;}
		if(u < min_u){min_u = u;}
		//ROS_INFO("duv: %f %f %f",d,u,v);
		/*
		ROS_INFO("Pos: %f %f %f",p->x,p->y,p->z);
		plane_points(0,0) = - p->x;
		plane_points(0,1) = - p->y;
		plane_points(0,2) = - p->z;
		cout <<"Mat:\n"<< plane_points.transpose()<<endl;
		MatrixXf tmp(1,3);
		tmp(0,0)= p->x-point_x;
		tmp(0,1)= p->y-point_y;
		tmp(0,2)= p->z-point_z;
		cout <<"Vec:\n"<< tmp.transpose() <<endl;
		
		MatrixXf TUV = (plane_points.transpose()).inverse()*(tmp.transpose());
		cout <<"TUV:\n"<< tmp.transpose() <<endl;
		*/
	}
	int max_resolution = 500;
	float diff_U = max_u-min_u;
	float diff_V = max_v-min_v;
	int U_res = 0;
	int V_res = 0;
	if(diff_U > diff_V){
		U_res = max_resolution;
		V_res = max_resolution*(diff_V/diff_U);
	}
	else{
		V_res = max_resolution;
		U_res = max_resolution*(diff_U/diff_V);
	}
	IplImage* img = cvCreateImage(cvSize(U_res,V_res),IPL_DEPTH_8U,3);
	
	for(int i = 0; i < U_res; i++)
	{
		for(int j = 0; j < V_res; j++)
		{
			img->imageData[3 * (j * U_res + i) + 0] = 0;
			img->imageData[3 * (j * U_res + i) + 1] = 0;
			img->imageData[3 * (j * U_res + i) + 2] = 0;
		}
	}
	
	for(int i = 0; i < size; i++)
	{
		Point * p = points->at(i);
		int w = ((float)U_res)*(U_pos[i]-min_u)/diff_U;
		int h = ((float)V_res)*(V_pos[i]-min_v)/diff_V;
		w = U_res-w;
		//h = V_res-h;
		img->imageData[3 * (h * U_res + w) + 0] = (unsigned char)255;
		img->imageData[3 * (h * U_res + w) + 1] = (unsigned char)255;
		img->imageData[3 * (h * U_res + w) + 2] = (unsigned char)255;
	}
	
	cvNamedWindow("plane projection", CV_WINDOW_AUTOSIZE );
	cvShowImage("plane projection", img);
	cvWaitKey(0);
	cvReleaseImage( &img );
	
	delete U_pos;
	delete V_pos;
}

PlaneNode::PlaneNode(){}
void PlaneNode::add(Point * p){}
